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1. INTRODUCTION 

A quadrotor is a six DOF Unmanned Aerial Vehicle (UAV) which consists of two pairs of counter-rotating 
rotors. Due to some unique abilities of quadrotor such as high maneuverability (hovering and VTOL), small size, and 
easy to control, quadrotor has been widely used. The usage area of a quadrotor can be separated into three major parts 
such as: military operations (security intelligence), public application (SAR), and civil application (photography). In 
order to improve the ability of quadrotor, some researchers were carry out on several aspect of control, such as path 
planning [1], position [2], altitude or hovering [3], and attitude [4]. 

Attitude of quadrotor is an orientation of the body fixed frame with respect to the inertial frame. It depends 
on the moment and the thrust of the two pairs of motor. By varying the rotor speed, one can change the lift force to 
create motion. The ability of quadrotor depends on an attitude control of quadrotor. If the attitude control is correct, 
then the additional task can be added to the system, for example, in the navigation task such as path planning, the 
orientation of quadrotor is an important to set the accuracy of sensor to detect the obstacle. The set of attitudes of 
quadrotor (rigid body) is the set of 3 x 3 orthogonal matrix whose determinant is one as known as Special Orthogonal 
SO(3)[5]. 

To control the attitude of quadrotor, many of control algorithms have been developed [6]. Most of them were 
developed with euler angles representation, which suffer from the problem of singularities for large angle rotational 
maneuvers[7] [8] [9] [10][11] [12]. To avoid singularity in euler angles representation, some works use a quaternion 
method to represent the attitude of quadrotor [13] [14] [15]. Although the quaternions do not increase to singularities, 
they have double cover of the set of attitudes SO(3) (no unique representation called ambiguities) in the sense that 
each attitude corresponds to two different quaternion vectors. Another work using a combination of Euler angles and 
unit-quaternion to represent the attitude of the aircraft [16]. 

One method that can avoid from singularities and ambiguities is a geometric control method, which uses a 
rotation matrix representation. This representation is global and unique. In geometric control method, the rotation 
matrix can be developed with an exponential matrix of exponential coordinate of an attitude, which map a vector R? 
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into a matrix SO(3) (Special orthogonal-3) by summing an infinite series of exponential skew-symmetric matrix. The 
exponential coordinate is one of the intrinsic properties of Lie group SO(3). 

Several studies to control a quadrotor based on exponential coordinate have been already done. In [17], a 
geometric tracking control system on SO(3) was provided to avoid singularities and ambiguities that are inherent to 
other attitude representation. This proposed tracking control also show its performance to stabilize the attitude error 
without the knowledge of the inertia matrix. In [18], a robust adaptive tracking control of the attitude dynamics of 
a rigid body was proposed. The controller made a tracking for an attitude and angular velocity command without 
knowledge of the inertia matrix of a rigid body. Both of these researches merely give the attitude tracking approach 
without considering the control torque. 

In [19], the attitude controller for a quadrotor based on exponential coordinate had been already done. The 
controller has been designed using TLC (trajectory linearisation control). The controller use an exponential coordi- 
nates Ç € R? and angular velocity w € R? as an error variable for controller. 

While in [19] uses a vector R? as an error variable, a rotation matrix error(Jt?**) as an error variable is used 
as an input of the controller in [20]. In [20], a PID algorithm is used and a knowledge of angular velocity error as an 
input to controller is needed. This work is look like focus in response time of the rotor. 

In this paper, a simple PD plus Feedforward controller on SO(3) with a rotation matrix R € SO(3) as an 
error variable for the system is proposed . This controller is simpler than the controller in [20]. This controller uses a 
proportional-derivative algorithm and only requires the measurements of angular velocity from dynamic of quadrotor 
to calculate the attitude. This controller can stabilize the attitude of quadrotor in hovering condition and making a 
motion to a desired attitude. 


2. RESEARCH METHOD 
In this section, some mathematical tools will be given for rotation parametrization and some basic knowledge 
of Lie Group and Lie Algebra. 


2.1. Mathematics of Rotation in SO(3) 


Rotation matrix is a 3 x 3 matrix that represents the transformation vector from a fixed-body frame to an 
inertial frame. Rotation matrix R € SO(3) is a Lie group which has an algebraic group structure based on matrix 
multiplication as the group operation. According to the Euler theorem for rotates body, any 3-dimensional rotation of 
a rigid body can be represented by a rotation of a given axis by an angle. Suppose that w € R? is a unit vector that 
specifying the rotation axis of a rigid body, and 0 € R is a rotation angle in radians. The position of a point at the 
rigid body as a function of time is denoted by q(t). If the body is rotated at constant unit velocity about the axis w, the 
velocity of the point can be written as 

a(t) = w x q(t) = å d(t). a) 


The hat operator ^ maps a vector in R? to a skew-symmetric matrix, defined as 


0 —W3 W2 
w= W3 0 TW . (2) 
—W2 Wy 0 


The inverse operation of the hat operator ^ is denoted as a vee operator Y, which recover a vector w € R? from a 
skew-symmetric matrix © € so(3). The skew-symmetric matrix 0 satisfies ©7@ = —@0. All such matrices forms a 
vector space denoted as so(3): 

so(3) = {9 € R?” : ST =—S}. (3) 


The exponential map transforms a skew-symmetric matrices into an orthogonal matrices. Geometrically, the skew- 
symmetric matrix corresponds to the axis of rotation (via the mapping w ++ w), and the exponential map generates the 
rotation corresponding to the rotation about the axis by a specified amount 0. So the exponential map exp : so(3) > 
SO(3) will maps a skew-symmetric matrix (20) to an orthogonal matrix (R = eĉ®), that is 
i . a 63. 
R = exp™? = I + 08 + —w2+ —w3+..., (4) 
2! 3! 
where J is a3 x 3 identity matrix, 0 is a rotation angle, and w is a rotation axis. 
The rotation matrix R € SO(3) is a three-dimensional special orthogonal matrix that satisfies 


RTR=I= RR", det(R) =1. (5) 


Attitude Control of Quadrotor Using PD Plus Feedforward ... (Almido H Ginting) 


568 ISSN: 2088-8708 


Equation (4) is an infinite series formula. To obtain a closed-form expression for exp(@6), by referred to Rodrigues’ 
Formula, an efficient method for computing exp(W6) is stated as 


R = exp“? = I + asin (0) + ĉ? (1 — cos 0), (6) 


where w = [w1 wz w3]! € RÌ, with ||w]| = 1 is the rotation axis, and @ is a rotation angle. The vector w0 € R? is 
the exponential coordinates ¢ for R € S'O(3), therefore, a rotation matrix can be calculated by an exponential matrix 
of a skew-symmetric matrix of exponential coordinate Ç, 


Ree, (7) 


The inverse mapping from an orthogonal matrix SO(3) to a skew-symmetric matrix so(3) is done by the 
logarithmic map which is defined as 


log(R) = R — R") € so(3), (8) 


a 
2 sin 0 


where @ is cos~! (==) and|0| < m, and tr(R) is the sum of the elements on the main diagonal of rotation 


matrix. If R = I, the rotation axis w can be chosen arbitrarily. And if R # J, the rotation axis can be calculated by 
log(R)Y, that is 


log(R)Y (R= RT) e R. (9) 


~ Qsind 


An illustration of a rotation with an axis and angle of rotation can be seen in Figure 1, where p = [p1, p2, pa] 
is a reference configuration, and t = |ti, t2, t3] is a present configuration. From Figure 1, a reference configuration 


p will be rotated by an angle 0 to a present configuration t with a rotation axis r. 
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Figure 1. Illustration of rotation with axis-angle 
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2.2. Quadrotor Model 


Figure 2. Model of quadrotor 


As shown in Figure 2, the inertial frame and body frame are denoted as Æ and B. The base of frame B can 
be represented using the axes of frame E, with coordinates X¢y,Yen,Zeb E R, where R? is a 3x1 dimensional-vector. 
In Figure 2, the quadrotor’s attitude is described using roll (9), pitch (¢), and yaw (Y) angle, written as Y = [0 dv". 
The angular velocity is described in body frame axes, denoted as Q% = [p qr]*. 

The inertia matrix of the quadrotor is as follows: 


Jrs Jey Jez 
E O N (10) 
Jez Jyz Jzz 


where the indices x, y, and z denote x—, y—, and z— axis in the body frame, respectively. If quadrotor is assumed to 
be symmetric to its x— and y— axis, the inertia matrix in (10) can be written as 


Jon 0 0 
aN) ayy ie Mh a1) 
0 O Jaz 


The inputs of quadrotor’s system are the squared angular speeds of its four rotors, denoted by W = [w? w3 w3 w? 


The rotors generate the torque T = [Tz Ty |" and thrus T. The torques and thrust are generated by manipulating the 
angular speed as 


I". 


Ta = db(w? — w3) 
Ty = db(w{ = w3) 
Tz = k(w} — w3 + w3 — w3) 


T = k(w? + w3 + w3 + wi), 


(12) 


where d, b, k are respectively the length of the quadrotor’s arm, propeller thrust coefficient, and propeller torque 
coefficient. 
The equation of motion of this quadrotor are given by 


R= Ro, (13) 
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Jw? +w? x Jw? = Tz (14) 


where J € R®*°, is the inertia matrix in the body fixed frame; w? € R?, is the angular velocity in the body frame; 
R € (SO(3), is the rotation matrix as a transform from the body frame to the inertial frame; tT € R3, is the control 
torque generated by the actuator of the robot. 

The dynamics of quadrotor in (14) can be rewritten as 


ww? = -J Hw x Jw?) + JIT, (15) 


and the quadrotor’s kinematics in (13), referred from [21], this differential equation can be rewritten in terms of the 
angle ¢ as 


5 T 2 b 
={|/J+-C+(l-a —,; ]u, 16 


where a(¢) = (¢/2)cot(¢/2), w? € R? is a body angular velocity, and ¢ € so(3) is an exponential coordinate of 
rotation matrix. Therefore, the second order system of quadrotor UAV in S'O(3) is defined as 


ie ĉ2 i 
=|I+- —a = |w 
Ç ( +56 +0- a (lcl) £) 


wo = JTH w x Jw?) + JTI. 


(17) 


2.3. PD Plus Feed-forward Control on SO(3) 


Based on the works of Bullo in [22], for a simple first order system on SO(3), a dynamical system with state 
g € G evolves following 
g=9V", (18) 


where G is a Lie Group matrix, g € SO(3) is the configuration of system, and V® is the body velocity. Assume that 
the quantity of V? € so(3) can be directly controlled to any desired value (i.e. the system is fully actuated). Then the 
proportional control action is 

V? = —kplog(g), kp > 0. (19) 


Now, consider the stabilization problem for second order systems, that is for the system where have full 
control over forces (accelerations) rather than velocities. A second order system on S'O(3) has the form 


. b 
g= 9K 
i , (20) 
Vè = f(g, V°) +U, 
where g € SO(3) is the configuration of the system, f(g, V®) € so(3) is the internal drift, and U € so(3) is the 
control input. To regulate the configuration g to the identity matrix J € SO(3), the proportional action will be coupled 
with a derivative term, i.e. with a term proportional to the velocity V®. Let kp and kg be symmetric, positive definite 
gains. Then the control law is 
U =—f(9,V") — kp log(g) — kaV?, (21) 


exponentially stabilizes the state g at Z € SO(3) from any initial condition tr(g(0)) — 1 and for all kp and V°(0) such 
that j 
Ivo] 


7? — |l90)ll306) 


Amin (kp) > ) (22) 


where Amin(kp) is the minimum eigenvalue of kp. 
Now, if the system is assumed as a quadrotor. And than, from the formulation above, if g € SO(3) is replaced 
with R € SO(3), V? with wè, and f(g,V°) € so(3) with f(Jw®, w°), then the control law for quadrotor is 


T =u? x Jw? — kp log(R) — kaw”, (23) 
where R € SO(38) is an attitude of quadrotor, w? 
and 7 is the control torque. 


is angular velocity in body frame, J is moment inertia of quadrotor 
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Figure 3. Block Diagram 
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Usually, PD control uses proportional and derivative error term. For example, if the objective is to control a 
variable y to match the constant set point yg with PD control, the control law v would be 


d(y = Ya) 
dt (24) 


v = —kp(y — ya) — ka 
= —kp(y = Ya) _ kay, 


where kp and kg are proportional and derivative constant, respectively. With this error term, if the log( R) is exponential 
coordinate of a rotation matrix Ç, the Equation (24) should be written as 


T =w” x Jw? — kp(C — Ca) — kaw”. (25) 


The block diagram of this controller is shown in Figure 3. From the block diagram of controller, the difference between 
rotation target matrix and current rotation matrix will be calculated in multiplication of matrix to get an error rotation 
matrix. With this multiply operation, the result is still in the form of a rotation matrix, which satisfy RT R = I = RRT 
and det(R) = 1. 


The rotation current matrix is calculated with the given angular velocity w from dynamic model. From 
[21], given body angular velocity w, the log(R) which consist of exponential coordinate of rotation matrix ¢ can be 
calculated using (7). 


3. RESULT AND ANALYSIS 

Here, the controller was simulated in 3 cases. In the first case, the controller is used to drive the attitude 
from an attitude condition to hovering condition. In the second case, the controller is used to control the attitude from 
hovering condition to a desired attitude. And in the last case, the simulation shows the ability of the controller to 
compensate an attitude disturbance. 

The initial and target of rotation matrix can be an any 3 x 3 matrix, where the matrix satisfies the condition 
in (5). The controller was simulated in 6 DOF euler angles with model parameter of quadrotor are created by Corke 
in [23], i.e. m = 4.34Kg, Jx = 0.082kg.m?, Jyy = 0.0845kg.m?2, Jz, = 0.1377kg.m?,b = 1.2953 x 10-°kg.m, 
d = 0.165m, k = 1.0368 x 10~*kg.m?. 


3.1. Case 1 


In this case, the simulation will show the performance of controller to drive an attitude toward a hovering 
condition (R14). In hovering condition, R1g will be a 3 x 3 identity matrix. In this case, the initial attitude of 
quadrotor Rinit 1S 

0.9956 0.06545 —0.06767 
Rinit = | —0.06767 0.9972 —0.03105 
0.06545 0.0355 0.9972 


The error of attitude Rerror can be calculated by multiplying the transpose matrix of desired attitude R14 with the 
initial attitude Rinit of quadrotor, that is 


T 


1 0 0 0.9956 0.06545 —0.06767 0.9956 0.06545 —0.06767 
Rerror = | 0 1 0 —0.06767 0.9972 —0.03105 | = | —0.06767 0.9972 —0.03105 
0 0 1 0.06545 0.0355 0.9972 0.06545 0.0355 0.9972 
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The exponential coordinates of the error of attitude become an input to the controller, to calculate the required torque 
for the system to achieve the desired attitude. The result of the simulation is shown in Figure 4. 


ANGULAR VELOCITY 
EXPONENTIAL COORDINATE OF ROTATION 0.02 T T T T T 


ROTATION ANGLE oes 
T T T 


7 Rotation angle target 
Rotation angle curent 


angle (rad) 


-0.02 
0.02 + 4 0.06 


Time (sec) 


ES 
(a) Rotation Angle (b) Exponential coordinate (c) Angular Velocity 


Figure 4. Simulation result of case 1 


In Figure 4b, the exponential coordinate of this case becomes [0 0 0]” when the system is convergent. This 
exponential coordinates vector will be transformed into a skew-symmetric matrix by operator ^, and the attitude of 
this condition can be calculated by exponential matrix of the skew-symmetric matrix of exponential coordinates as 
shown in (7). 


0 —0 0 1 0 0 
Rig = exp 0 0 -O;=j;0 1 0 
—0 0 0 0 0 1 


The condition describes that the quadrotor has no rotation or in hovering condition, the same as the rotation target in 
this case. The rotation angle is 0 rad as shown in Figure 4a. In this condition, while the attitude R = J, the rotation 
axis w can be chosen arbitrarily. When the system meets convergence, the angular velocity for all axis are [0 0 0]” as 
shown in Figure 4c. The steady state in this system is reached in 3 sec with the rise time is about 1 sec. 


3.2. Case 2 


In the second case, the simulation will show the performance of controller to control the quadrotors attitude 
from hovering condition to a desired attitude. In this case, the initial attitude Rinit is a 3 x 3 identity matrix and the 
desired attitude R2, is 

0.36 0.48 —0.8 
R2¢4= | —0.8 0.6 0 
0.48 0.64 0.60 


The error of attitude Rerror in this second case can be calculated by multiplying the transpose matrix of 
desired attitude R24 with the initial attitude Rinit of quadrotor , that is 
0.36 048 -08]/[1 0 0 0.36 —0.80 0.48 
Rerror = | —0.8 0.6 0 0 1 0)/= 0.48 0.60 0.64 
0.48 0.64 0.60 0 0 1 —0.80 0 0.6 


The result of the second case is shown in Figure 5. When the system is convergent, the value of exponential 


coordinate are [0.429 — 0.858 —0.858]7 (see Figure 5b), and the attitude can be calculated by the same way as shown 
in Case 1, that is 
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0 0.858 —0.858 0.36 0.48 —0.8 
R24 = exp | —0.858 0 —0.429 | = | —0.8 0.6 0 
0.8580 0.429 0 0.48 0.64 0.6 


Exponential Coordinate of Rotation 
ROTATION ANGLE 4 — ia 
14 T T T T T T T T T 


7 angetarget 
angle current 


Angle (rad) 


L L L L 
0 1 2 3 4 5 6 7 a 9 10 ee 
Time (sec) 4 L L È 1 l L 1 L ni 

° 1 2 3 4 5 € T a s w Time (sec) 


(a) Rotation Angle (b) Exponential coordinate (c) Angular Velocity 


Figure 5. Simulation Result of case 2 


As shown in Figure 5a and the value of R24, the rotation axis can be calculated by (9), that is [0.3333 — 
0.6667 — 0.6667]". This means that the quadrotor is rotated with this rotation axis, and rotates about 1.287 rad (73.4 
degree). The value of angular velocity in Figure 5c indicates that there is no alteration of rotation angle when the 
system is convergent. 


3.3. Case 3 


In the third case, the simulation is done to test the controller’s ability to compensate the disturbance of the 
system. The controller is simulated with disturbance by [0.1 0.1 0.1)” in attitude current Re, with the initial and target 
of attitude is the same as in Case 2. In this case, this controller can preserve the value of attitude current in 3.03 sec 
as shown in Figure 6a. When a disturbance is added to the system, the rotation error raises by 0.06 point in x-axis 
and the controller drives the system to make the error of attitude become zero in 3.03 sec as shown in Figure 6b. As 
long as the system is added by a disturbance, the controller preserves error in zero point. When the disturbance is 
eliminated from the system, rotation error in this system is reduced from zero point to point -0.06. In about 3 sec, the 
controller can drive the the system to the desired attitude, and the rotation error becomes zero when the system meets 
convergence. 
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Figure 6. Simulation Result of case 3 


Based on all of the cases, rotation error matrix is a 3 x 3 identity matrix when the system is convergent. The 
steady state time is about 3 sec and the rise time is about | sec for all cases. 


4. CONCLUSION 


A new simple scheme of PD plus Feedforward controller on SO(3) to control the attitude of quadrotor is 
proposed in this paper. This controller uses a rotation matrix error as a variable error of the system. The controller is 
able to drive the attitude of quadrotor from hovering condition to desired attitude and from an attitude condition goes 
to the hovering condition, with the convergence time of about 3 sec. The value of angular velocity will be zero when 
the desired equilibrium is achieved and the rotation error matrix will be a 3 x 3 identity matrix. This controller can 
also drive the system to the desired attitude despite the system is disturbed. For the future work, this controller will be 
implemented to control the maneuvering of quadrotor in the real quadrotor. 
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